import cv2 as cv
import numpy as np




class fineTuner():
    def __init__(self,controller):
        self.controller = controller

    def fineTuning(self,img,test=False):
        gray = cv.cvtColor(img, cv.COLOR_BGR2GRAY)
        edges = cv.Canny(gray, 50, 150, apertureSize=3)
        yuzhi = 200
        while yuzhi > 100:
            print('霍弗变换当前阈值：'+str(yuzhi))
            lines = cv.HoughLines(edges, 1, np.pi/180, yuzhi)#原为200
            if type(lines) == type(None):
                yuzhi -= 20
            else:
                break
        if type(lines) == type(None):
            return  [0,0,0]

        dist = 0
        hengDist = 0
        hengDistTemp = 0
        angle = 0
        for line in lines:
            rho,theta = line[0]

            if np.abs(theta-np.pi/2) < 0.3:
                d0 = 480 - rho
                if d0 < dist or dist == 0:
                    dist = d0
                    angle = np.abs(theta)
                print("距离：%s"%d0)

            if theta > np.pi * 0.7:
                theta -= np.pi
            if np.abs(theta) < 0.2*np.pi:
                d0 = np.abs(rho)
                if d0 < 320:
                    hengDistTemp = - d0/50 - 3
                    print('需要向右调整')
                else:
                    d0 = 640 - d0
                    hengDistTemp = d0 / 50 + 3

                if np.abs(hengDistTemp) > np.abs(hengDist):
                    hengDist = hengDistTemp




        print("final距离：%s" % dist)

        for line in lines:
            rho, theta = line[0]
            a = np.cos(theta)
            b = np.sin(theta)
            x0 = a * rho
            y0 = b * rho
            x1 = int(x0 + 1000 * (-b))
            y1 = int(y0 + 1000 * (a))
            x2 = int(x0 - 1000 * (-b))
            y2 = int(y0 - 1000 * (a))

            cv.line(img, (x1, y1), (x2, y2), (0, 0, 255), 2)

        cv.imwrite('houghlines3.jpg', img)

        return [dist/50*0.8,hengDist,angle]

        # if dist > 200 and not test:
        #     self.controller.goForword(dist/100)
        #     return True
        # else:
        #     return False


if __name__ == "__main__":
    img = cv.imread('img/98.bmp')
    f = fineTuner(None)
    f.fineTuning(img,True)






